
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_mode.c
  * @author     baiyang
  * @date       2021-8-16
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <parameter/param.h>
#include <logger/blog_msg.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
void fms_set_accel_throttle_I_from_pilot_throttle();
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// return the static controller object corresponding to supplied mode
Mode *fms_mode_from_mode_num(const ModeNumber mode)
{
    Mode *ret = NULL;

    switch (mode) {

        case STABILIZE:
            ret = fms.mode_stabilize;
            break;
        case ALT_HOLD:
            ret = fms.mode_althold;
            break;
        default:
            break;
    }

    return ret;
}

// called when an attempt to change into a mode is unsuccessful:
void fms_mode_change_failed(const Mode *mode, const char *reason)
{
    mavproxy_send_statustext(MAV_SEVERITY_WARNING, "Mode change to %s failed: %s", mode->name(), reason);
    //AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode->mode_number()));
    // make sad noise
    if (fms.ap.initialised) {
        //AP_Notify::events.user_mode_change_failed = 1;
    }
}

// set_mode - change flight mode and perform any necessary initialisation
// optional force parameter used to force the flight mode change (used only first time mode is set)
// returns true if mode was successfully set
// ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
bool fms_set_mode(ModeNumber mode, ModeReason reason)
{
    // update last reason
    const ModeReason last_reason = fms._last_reason;
    fms._last_reason = reason;

    // return immediately if we are already in the desired mode
    if (mode == fms.flightmode->mode_number()) {
        fms.control_mode_reason = reason;
        // make happy noise
        if (fms.ap.initialised && (reason != last_reason)) {
            //AP_Notify::events.user_mode_change = 1;
        }
        return true;
    }

#if 0
#if MODE_AUTO_ENABLED == ENABLED
    if (mode == Mode::Number::AUTO_RTL) {
        // Special case for AUTO RTL, not a true mode, just AUTO in disguise
        return mode_auto.jump_to_landing_sequence_auto_RTL(reason);
    }
#endif
#endif

    Mode *new_flightmode = fms_mode_from_mode_num((ModeNumber)mode);
    if (new_flightmode == NULL) {
        mavproxy_send_statustext(MAV_SEVERITY_WARNING,"No such mode");
        //AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
        return false;
    }

    bool ignore_checks = !fms.motors->_armed;   // allow switching to any mode if disarmed.  We rely on the arming check to perform

#if FRAME_CONFIG != HELI_FRAME
    // ensure vehicle doesn't leap off the ground if a user switches
    // into a manual throttle mode from a non-manual-throttle mode
    // (e.g. user arms in guided, raises throttle to 1300 (not enough to
    // trigger auto takeoff), then switches into manual):
    bool user_throttle = new_flightmode->has_manual_throttle();
    if (!ignore_checks &&
        fms.ap.land_complete &&
        user_throttle &&
        !fms.flightmode->has_manual_throttle() &&
        fms_get_pilot_desired_throttle() > fms_get_non_takeoff_throttle()) {
        fms_mode_change_failed(new_flightmode, "throttle too high");
        return false;
    }
#endif

    if (!ignore_checks &&
        new_flightmode->requires_GPS() &&
        !fms_position_ok()) {
        fms_mode_change_failed(new_flightmode, "requires position");
        return false;
    }

    // check for valid altitude if old mode did not require it but new one does
    // we only want to stop changing modes if it could make things worse
    if (!ignore_checks &&
        !fms_ekf_alt_ok() &&
        fms.flightmode->has_manual_throttle() &&
        !new_flightmode->has_manual_throttle()) {
        fms_mode_change_failed(new_flightmode, "need alt estimate");
        return false;
    }

    if (!new_flightmode->init(ignore_checks)) {
        fms_mode_change_failed(new_flightmode, "initialisation failed");
        return false;
    }

    // perform any cleanup required by previous flight mode
    fms_exit_mode(fms.flightmode, new_flightmode);

    // store previous flight mode (only used by tradeheli's autorotation)
    fms.prev_control_mode = fms.flightmode->mode_number();

    // update flight mode
    fms.flightmode = new_flightmode;
    fms.control_mode_reason = reason;

    // 在日志中记录飞行模式
    blog_write_mode(fms.flightmode->mode_number(), reason);
    
    //gcs().send_message(MSG_HEARTBEAT);

    // update notify object
    fms_notify_flight_mode();

    // make happy noise
    if (fms.ap.initialised) {
        //AP_Notify::events.user_mode_change = 1;
    }

    // return success
    return true;
}

// set_mode - change flight mode and perform any necessary initialisation
// optional force parameter used to force the flight mode change (used only first time mode is set)
// returns true if mode was successfully set
// ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
bool fms_set_mode_rc(int8_t new_pos, int8_t reason)
{
    ModeNumber mode = (ModeNumber)fms.flight_modes[new_pos];

    // update last reason
    const ModeReason last_reason = fms._last_reason;
    fms._last_reason = reason;

    // return immediately if we are already in the desired mode
    if (mode == fms.flightmode->mode_number()) {
        fms.control_mode_reason = reason;
        // make happy noise
        if (fms.ap.initialised && (reason != last_reason)) {
            //AP_Notify::events.user_mode_change = 1;
        }
        return true;
    }

#if 0
#if MODE_AUTO_ENABLED == ENABLED
    if (mode == Mode::Number::AUTO_RTL) {
        // Special case for AUTO RTL, not a true mode, just AUTO in disguise
        return mode_auto.jump_to_landing_sequence_auto_RTL(reason);
    }
#endif
#endif

    Mode *new_flightmode = fms_mode_from_mode_num((ModeNumber)mode);
    if (new_flightmode == NULL) {
        mavproxy_send_statustext(MAV_SEVERITY_WARNING,"No such mode");
        //AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
        return false;
    }

    bool ignore_checks = !fms.motors->_armed;   // allow switching to any mode if disarmed.  We rely on the arming check to perform

#if FRAME_CONFIG != HELI_FRAME
    // ensure vehicle doesn't leap off the ground if a user switches
    // into a manual throttle mode from a non-manual-throttle mode
    // (e.g. user arms in guided, raises throttle to 1300 (not enough to
    // trigger auto takeoff), then switches into manual):
    bool user_throttle = new_flightmode->has_manual_throttle();
    if (!ignore_checks &&
        fms.ap.land_complete &&
        user_throttle &&
        !fms.flightmode->has_manual_throttle() &&
        fms_get_pilot_desired_throttle() > fms_get_non_takeoff_throttle()) {
        fms_mode_change_failed(new_flightmode, "throttle too high");
        return false;
    }
#endif

    if (!ignore_checks &&
        new_flightmode->requires_GPS() &&
        !fms_position_ok()) {
        fms_mode_change_failed(new_flightmode, "requires position");
        return false;
    }

    // check for valid altitude if old mode did not require it but new one does
    // we only want to stop changing modes if it could make things worse
    if (!ignore_checks &&
        !fms_ekf_alt_ok() &&
        fms.flightmode->has_manual_throttle() &&
        !new_flightmode->has_manual_throttle()) {
        fms_mode_change_failed(new_flightmode, "need alt estimate");
        return false;
    }

    if (!new_flightmode->init(ignore_checks)) {
        fms_mode_change_failed(new_flightmode, "initialisation failed");
        return false;
    }

    // perform any cleanup required by previous flight mode
    fms_exit_mode(fms.flightmode, new_flightmode);

    // store previous flight mode (only used by tradeheli's autorotation)
    fms.prev_control_mode = fms.flightmode->mode_number();

    // update flight mode
    fms.flightmode = new_flightmode;
    fms.control_mode_reason = reason;

    // 在日志中记录飞行模式
    blog_write_mode(fms.flightmode->mode_number(), reason);

    //gcs().send_message(MSG_HEARTBEAT);

    // update notify object
    fms_notify_flight_mode();

    // make happy noise
    if (fms.ap.initialised) {
        //AP_Notify::events.user_mode_change = 1;
    }

    // return success
    return true;
}

// update_flight_mode - calls the appropriate attitude controllers based on flight mode
// called at 100hz or more
void fms_update_flight_mode()
{
    //surface_tracking.invalidate_for_logging();  // invalidate surface tracking alt, flight mode will set to true if used

    fms.flightmode->run();
}

// exit_mode - high level call to organise cleanup as a flight mode is exited
void fms_exit_mode(Mode *old_flightmode,
                       Mode *new_flightmode)
{
    // smooth throttle transition when switching from manual to automatic flight modes
    if (old_flightmode->has_manual_throttle() && !new_flightmode->has_manual_throttle() && fms.motors->_armed && !fms.ap.land_complete) {
        // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
        fms_set_accel_throttle_I_from_pilot_throttle();
    }

#if 0
    // cancel any takeoffs in progress
    old_flightmode->takeoff_stop();
#endif

    // perform cleanup required for each flight mode
    old_flightmode->exit();
}

// notify_flight_mode - sets notify object based on current flight mode.  Only used for OreoLED notify device
void fms_notify_flight_mode() 
{
    //AP_Notify::flags.autopilot_mode = flightmode->is_autopilot();
    //AP_Notify::flags.flight_mode = (uint8_t)flightmode->mode_number();
    //notify.set_flight_mode_str(flightmode->name4());
}

float fms_throttle_hover()
{
    return ((MotorsMC_HandleTypeDef *)fms.motors)->_throttle_hover;
}

// get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff
float fms_get_non_takeoff_throttle()
{
    return MAX(0,((MotorsMC_HandleTypeDef *)fms.motors)->_throttle_hover/2.0f);
}

// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
void fms_get_pilot_desired_lean_angles(float *roll_out, float *pitch_out, float angle_max, float angle_limit)
{
    // throttle failsafe check
    if (fms.failsafe.radio || !fms.ap.rc_receiver_present) {
        *roll_out = 0;
        *pitch_out = 0;
        return;
    }
    // fetch roll and pitch inputs
    float roll_in = fms.channel_roll->control_in;
    float pitch_in = fms.channel_pitch->control_in;

    // limit max lean angle
    angle_limit = math_constrain_float(angle_limit, 1000.0f, angle_max);

    // scale roll and pitch inputs to ANGLE_MAX parameter range
    float scaler = angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX;
    roll_in *= scaler;
    pitch_in *= scaler;

    // do circular limit
    float total_in = sqrtf(pitch_in*pitch_in + roll_in*roll_in);
    if (total_in > angle_limit) {
        float ratio = angle_limit / total_in;
        roll_in *= ratio;
        pitch_in *= ratio;
    }

    // do lateral tilt to euler roll conversion
    roll_in = (18000/M_PI) * atanf(cosf(pitch_in*(M_PI/18000))*tanf(roll_in *(M_PI/18000)));

    // roll_out and pitch_out are returned
    *roll_out = roll_in;
    *pitch_out = pitch_in;
}

// transform pilot's manual throttle input to make hover throttle mid stick
// used only for manual throttle modes
// thr_mid should be in the range 0 to 1
// returns throttle output 0 to 1
float fms_get_pilot_desired_throttle()
{
    const float thr_mid = ((MotorsMC_HandleTypeDef *)fms.motors)->_throttle_hover;
    int16_t throttle_control = fms.channel_throttle->control_in;

    int16_t mid_stick = RC_get_control_mid(fms.channel_throttle);
    // protect against unlikely divide by zero
    if (mid_stick <= 0) {
        mid_stick = 500;
    }

    // ensure reasonable throttle values
    throttle_control = math_constrain_int16(throttle_control,0,1000);

    // calculate normalised throttle input
    float throttle_in;
    if (throttle_control < mid_stick) {
        throttle_in = ((float)throttle_control)*0.5f/(float)mid_stick;
    } else {
        throttle_in = 0.5f + ((float)(throttle_control-mid_stick)) * 0.5f / (float)(1000-mid_stick);
    }

    const float expo = math_constrain_float(-(thr_mid-0.5f)/0.375f, -0.5f, 1.0f);
    // calculate the output throttle using the given expo function
    float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in;
    return throttle_out;
}

// get_pilot_desired_climb_rate - transform pilot's throttle input to climb rate in cm/s
// without any deadzone at the bottom
float fms_get_pilot_desired_climb_rate(float throttle_control)
{
    // throttle failsafe check
    if (fms.failsafe.radio || !fms.ap.rc_receiver_present) {
        return 0.0f;
    }

    int16_t throttle_deadzone = PARAM_GET_INT16(VEHICLE, THR_DZ);

    // ensure a reasonable throttle value
    throttle_control = math_constrain_float(throttle_control,0.0f,1000.0f);

    // ensure a reasonable deadzone
    throttle_deadzone = math_constrain_int16(throttle_deadzone, 0, 400);

    float desired_rate = 0.0f;
    const float mid_stick = RC_get_control_mid(fms.channel_throttle);
    const float deadband_top = mid_stick + throttle_deadzone;
    const float deadband_bottom = mid_stick - throttle_deadzone;

    // check throttle is above, below or in the deadband
    if (throttle_control < deadband_bottom) {
        // below the deadband
        desired_rate = fms_get_pilot_speed_dn() * (throttle_control-deadband_bottom) / deadband_bottom;
    } else if (throttle_control > deadband_top) {
        // above the deadband
        desired_rate = fms_get_pilot_speed_up() * (throttle_control-deadband_top) / (1000.0f-deadband_top);
    } else {
        // must be in the deadband
        desired_rate = 0.0f;
    }

    return desired_rate;
}

// transform pilot's yaw input into a desired yaw rate
// returns desired yaw rate in centi-degrees per second
float fms_get_pilot_desired_yaw_rate(int16_t stick_angle)
{
    float acro_y_expo = PARAM_GET_FLOAT(VEHICLE,ACRO_Y_EXPO);
    float acro_yaw_p = PARAM_GET_FLOAT(VEHICLE,ACRO_YAW_P);

    // throttle failsafe check
    if (fms.failsafe.radio || !fms.ap.rc_receiver_present) {
        return 0.0f;
    }

    // range check expo
    acro_y_expo = math_constrain_float(acro_y_expo, -0.5f, 1.0f);

    // calculate yaw rate request
    float yaw_request;
    if (math_flt_zero(acro_y_expo)) {
        yaw_request = stick_angle * acro_yaw_p;
    } else {
        // expo variables
        float y_in, y_in3, y_out;

        // yaw expo
        y_in = (float)stick_angle/ROLL_PITCH_YAW_INPUT_MAX;
        y_in3 = y_in*y_in*y_in;
        y_out = (acro_y_expo * y_in3) + ((1.0f - acro_y_expo) * y_in);
        yaw_request = ROLL_PITCH_YAW_INPUT_MAX * y_out * acro_yaw_p;
    }
    // convert pilot input to the desired yaw rate
    return yaw_request;
}

// set_accel_throttle_I_from_pilot_throttle - smoothes transition from pilot controlled throttle to autopilot throttle
void fms_set_accel_throttle_I_from_pilot_throttle()
{
    // get last throttle input sent to attitude controller
    float pilot_throttle = math_constrain_float(attctrl_get_throttle_in(fms.attitude_control), 0.0f, 1.0f);
    // shift difference between pilot's throttle and hover throttle into accelerometer I

    pid_ctrl_set_integrator1(&(fms.pos_control->_pid_accel_z), (pilot_throttle-MotorsMC_get_throttle_hover((MotorsMC_HandleTypeDef *)fms.motors) * 1000.0f));
}

// It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
uint16_t fms_get_pilot_speed_dn()
{
    if (PARAM_GET_INT16(VEHICLE, PILOT_SPEED_DN) == 0) {
        return abs(PARAM_GET_INT16(VEHICLE, PILOT_SPEED_UP));
    } else {
        return abs(PARAM_GET_INT16(VEHICLE, PILOT_SPEED_DN));
    }
}
/*------------------------------------test------------------------------------*/


